#define ONTIMER_LIMIT 1000
#define ONTIMER_THRESH 500
void
callback ()
{
  //sample current limits at highest rate
  aCurrent = analogReadMulti (5);
  bCurrent = analogReadMulti (4);
  uCommand = (analogReadMulti (0) - 512) * 16;
  int uCurRabbit = uRabbit;
  //lowest is rabbit and battery
  uRabbit = analogReadMulti (1);
  //battery should be last.  
  //It slowly changes so is a good indicator of analog input quality
  batVoltage = (analogReadMulti (2) * 10) / 34;
  static char lastDidHeadLights = 0;
  char didHeadlights = digitalRead (READHEADLIGHTS);
  digitalWrite (DEBUGLIGHT, didHeadlights);
  if (lastDidHeadLights != didHeadlights)
    {
      if (didHeadlights)
	digitalWrite (HEADLIGHTS, digitalRead (HEADLIGHTS) ^ 1);
      lastDidHeadLights = didHeadlights;
    }
  //subtract 4xcurrent
  if (motorState >= IDLE)
    {
      if ((uCommand > 1600) || (uCommand < -1600))
	{
	  if (onTimer < 0)
	    onTimer = ONTIMER_THRESH;
	  onTimer++;
	}
      else
	onTimer--;
    }
  else
    {
      if (IS_REVERSE (motorState))
	{
	  if (uCommand < -1600)
	    {
	      if (onTimer < 0)
		onTimer = ONTIMER_THRESH;
	      onTimer++;
	    }
	  else
	    onTimer--;
	}

      else
	{
	  if (uCommand > 1600)
	    {
	      if (onTimer < 0)
		onTimer = ONTIMER_THRESH;
	      onTimer++;
	    }
	  else
	    onTimer--;
	}

    }
  if (IS_PWM (motorState))
    {
      //0 values for pwm prevent regen
      if (IS_REVERSE (motorState))
	{
	  if (uCommandFiltered > -80)
	    uCommandFiltered = -80;
	}
      else
	{
	  if (uCommandFiltered < 80)
	    uCommandFiltered = 80;
	}
    }
  else
    {
      //prevent high initial values
      uCommandFiltered = 0;
    }
  switch (motorState)
    {
    case FORWARD:		//pwm on,brake off
      if (onTimer < -ONTIMER_LIMIT)
	motorState = ENABLE_FWD;
      Timer1.pwm (PWMA, uCommandFiltered / 8);
      digitalWrite (PWMPOWER, 1);
      break;
    case REVERSE:		//pwm on,brake off
      if (onTimer < -ONTIMER_LIMIT)
	motorState = ENABLE_REV;
      //0 values for pwm prevent regen
      Timer1.pwm (PWMA, 1024 + uCommandFiltered / 16);
      digitalWrite (PWMPOWER, 1);
      break;
    case ENABLE_FWD:		//pwm on (low duty cycle),brake off
      if (onTimer > ONTIMER_LIMIT)
	motorState = FORWARD;
      if (onTimer < -ONTIMER_LIMIT)
	motorState = PWMOFF_FWD;
      digitalWrite (PWMREVERSE, 0);
      digitalWrite (PWMPOWER, 1);
      Timer1.pwm (PWMA, uCommandFiltered / 8);
      break;
    case ENABLE_REV:		//pwm on (low duty cycle),brake off
      if (onTimer > ONTIMER_LIMIT)
	motorState = REVERSE;
      if (onTimer < -ONTIMER_LIMIT)
	motorState = PWMOFF_REV;
      Timer1.pwm (PWMA, 1024 + uCommandFiltered / 16);
      digitalWrite (PWMREVERSE, 1);
      digitalWrite (PWMPOWER, 1);
      break;
    case PWMOFF_FWD:		//pwm off ,brake off
      if (onTimer > ONTIMER_LIMIT)
	motorState = ENABLE_FWD;
      if (onTimer < -ONTIMER_LIMIT)
	motorState = PWROFF_FWD;
      digitalWrite (PWMENABLE, 0);
      digitalWrite (PWMREVERSE, 0);
      digitalWrite (PWMPOWER, 1);
      Timer1.pwm (PWMA, 0);
      break;
    case PWMOFF_REV:		//pwm off brake off
      if (onTimer > ONTIMER_LIMIT)
	motorState = ENABLE_REV;
      if (onTimer < -ONTIMER_LIMIT)
	motorState = PWROFF_REV;
      digitalWrite (PWMENABLE, 0);
      Timer1.pwm (PWMA, 1024);
      digitalWrite (PWMREVERSE, 1);
      digitalWrite (PWMPOWER, 1);
      break;
    case PWROFF_FWD:		//pwm off ,brake off
      if (onTimer > ONTIMER_LIMIT)
	motorState = PWMOFF_FWD;
      if (onTimer < -ONTIMER_LIMIT)
	{
	  if (uCommand < -300)
	    motorState = PWROFF_REV;
	  else
	    motorState = IDLE;
	}
      digitalWrite (PWMENABLE, 0);
      digitalWrite (PWMREVERSE, 0);
      digitalWrite (PWMPOWER, 0);
      Timer1.pwm (PWMA, 0);
      break;
    case PWROFF_REV:		//pwm off brake off
      if (onTimer > ONTIMER_LIMIT)
	motorState = PWMOFF_REV;
      if (onTimer < -ONTIMER_LIMIT)
	{
	  //      if(uCommand>100)
	  //        motorState=PWMOFF_FWD;
	  //      else
	  motorState = IDLE;
	}
      digitalWrite (PWMENABLE, 0);
      Timer1.pwm (PWMA, 1024);
      digitalWrite (PWMREVERSE, 1);
      digitalWrite (PWMPOWER, 0);
      break;
    case IDLE:			//turn off pwm board
      digitalWrite (BRAKE_RELEASE, 0);
      digitalWrite (PWMPOWER, 0);
      if (onTimer > ONTIMER_LIMIT)
	{
	  if (uCommand > 100)
	    {
	      digitalWrite (BRAKE_RELEASE, 1);
	      motorState = PWROFF_FWD;
	    }
	  else
	    {
	      digitalWrite (BRAKE_RELEASE, 1);
	      motorState = PWROFF_REV;
	    }
	}
      break;
      //this forces things safe on any out-of-bounds value
    default:
      motorState = FAULTED;
      break;
    }
  //cleanup OnTimer
  if (onTimer > ONTIMER_LIMIT)
    onTimer = ONTIMER_THRESH;
  if (onTimer < -ONTIMER_LIMIT)
    onTimer = -ONTIMER_THRESH;
  //limit pwm
  if (IS_LOWDUTYCYCLE (motorState))
    uCurRabbit = 0;
  if (motorState != IDLE)
    {
      //current limiting
      if (0)
	{
	  if (aCurrent > 300)
	    {
	      uCurrentLimit++;
	      uCurRabbit = 64;
	    }
	  if (bCurrent > 300)
	    {
	      uCurrentLimit++;
	      uCurRabbit = 64;
	    }
	}
    }
  //rabbit limiting
  long scaledNextCommand = (uCurRabbit / 16) + 8;	//16 levels of rabbit (4 bits)
  //if(  uNextCommand>16)
  //uNextCommand=16;
  scaledNextCommand = long (uCommand) * scaledNextCommand;
  uCommandLong = ((uCommandLong * 127L) + (scaledNextCommand >> 6)) >> 7;
  if (uCommandLong > 512 * 16)
    uCommandLong = 512 * 16;
  if (uCommandLong < -511 * 16)
    uCommandLong = -511 * 16;
  uCommandFiltered = uCommandLong;
  DebugValue = uCommandFiltered;
  digitalWrite (PWMENABLE, IS_PWM (motorState));
  digitalWrite (8, digitalRead (8) ^ 1);
  //  static char counter=0;
}



